/*
 * main.cpp
 *
 *  Created on: Nov 22, 2012
 *      Author: bouchier
 */

/*! \file JRoverSensors/main.cpp
 * \brief Header file for jRoverSensors - the file which does whole-system exercising of jRover Sensors
 *
 * <H1>
 * Build Configuration
 * </H1>
 *
 * This project depends on the following peer projects being at the same directory level:
 * - Metro
 * - RCTest
 * - usonicRanger
 * - ControlledMotor
 * - CQEI2C
 * - CQEIMEncoder
 * - PID
 *
 * Edit the project properties as follows to reference them as includes, link objects, and referenced projects.
 *
 * Under C/C++ Build -> Settings, Tool Settings tab, TerkOS C++ Compiler group, Directories settings
 * add the following paths
 * to the terkos paths that are already there. This adds them to the include path for compilation
 * - ../../Metro
 * - ../../RCTest
 * - ../../ControlledMotor
 *
 * Under C/C++ Build -> Settings, Tool Settings tab, TerkOS C++ Linker group, Miscellaneous settings, add
 * the following "other objects". This tells the linker to link to qetime.o & CQEI2C.o.
 * - ../../Metro/Debug/Metro.o
 * - ../../qetime/Debug/qetime.o
 * - ../../RCTest/Debug/RCTest.o
 * - ../../ControlledMotor/Debug/ControlledMotor.o
 * - ../../CQEI2C/Debug/CQEI2C.o
 * - ../../CQEIMEncoder/Debug/CQEIMEncoder.o
 * - ../../PID/Debug/pid.o
 *
 * In the Project References group, check the following projects. This builds them before the current project.
 * Metro
 * qetime
 * RCTest
 * ControlledMotor
 * CQEI2C
 * CQEIMEncoder
 * PID
 */
#define DEFAULT_TEST 1

#include <stdio.h>
#include <stdlib.h>
#include <qegpioint.h>
#include <qeanalog.h>
#include <CQEI2C.h>
#include <ControlledMotor.h>
#include "Metro.h"
#include "JRoverSensors.h"

CQEI2C i2c = CQEI2C();		// instantiate the I2C driver
CQEGpioInt &gpio = CQEGpioInt::GetRef();
CQEAnalog &analog = CQEAnalog::GetRef();
JRoverSensors jRoverSensors = JRoverSensors();

#define RM_DRIVE 12
#define LM_DRIVE 2
#define DRIVE_SMOTOR_KP 10.0
#define DRIVE_SMOTOR_KI 1.0
#define DRIVE_SMOTOR_KD 0.0

#define PI 3.14159265359
#define WHEEL_CIRCUMFERENCE 2 * PI * 2
ControlledMotor lmDrive = ControlledMotor(LM_DRIVE, i2c, true, WHEEL_CIRCUMFERENCE);
ControlledMotor rmDrive = ControlledMotor(RM_DRIVE, i2c, false, WHEEL_CIRCUMFERENCE);

float rosLinear;
float rosAngular;

void usage()
{
	printf("Usage: JRoverSensors testNum\n");
	printf("testNum can be:\n");
	printf("1: print pulse-widths, ranges, & batt voltage\n");
}

void test1()
{
	printf("Running test 1\n");
	float voltage;

	jRoverSensors.initSensors();
	Metro metro = Metro(50);
	while (1){
		if (metro.check()) {
			jRoverSensors.pollSensors();
			voltage = jRoverSensors.getBatteryVoltage();
			printf("Speed: %d, Dir: %d, estop: %d, battery: %0.2f\n",
					jRoverSensors.getRcSpeed(), jRoverSensors.getRcDir(), jRoverSensors.getRcEstop(),
					voltage);
		}
	}
}

void test2()
{
	printf("Running test 2\n");

	jRoverSensors.initSensors();
	lmDrive.init(DRIVE_SMOTOR_KP, DRIVE_SMOTOR_KI, DRIVE_SMOTOR_KD);
	rmDrive.init(DRIVE_SMOTOR_KP, DRIVE_SMOTOR_KI, DRIVE_SMOTOR_KD);

	Metro metro = Metro(50);
	Metro metro250 = Metro(250);

	while (1){
		if (metro.check()) {
			jRoverSensors.pollSensors();
		}

		if (metro250.check()) {
			printf("Rangers: L: %d, C: %d, R: %d, battery: %0.2f\n",
					jRoverSensors.getLDistance(),jRoverSensors.getCDistance(), jRoverSensors.getRDistance(),
					jRoverSensors.getBatteryVoltage());
		}
	}
}

int main(int argc, char **argv)
{
	int test = DEFAULT_TEST;	// which test to run
	int argval;		// value to pass into test

	if (argc == 2)
		test = atoi(argv[1]);
	if (argc == 3)
		argval = atoi(argv[2]);

	switch (test) {
	case 1:	test1(); break;
	case 2: test2(); break;
	//case 3: test3(); break;
	//case 4: test4(); break;
	default:
		printf("Invalid option\n");
		usage();
	}
}
